# -*- coding:utf-8 -*-

# satellite class maintain satellite data, such as clock, state matrix and inter-satellite action

from  builtins import range
from functools import reduce
import numpy as np
from cnode import cNode
from cmsg import *
import logging
from cgraph import *  # recursively import happen, it can be wrong.
from satProp import BDT
from hybridNode import *
from satProp import *
from ctypes import *

import time
# TODO add other state
class cSat(object):
    '''
        class cSat maintain a satellite's state.
        timeDrift: time drift w.r.t the true time, unit: second
    '''
    def __init__(self,name=None,kineState=None,timeDriftState=None,backwardMode=None,propagator= None):
        if name is None:
            name='asat'
        if kineState is None:
            kineState=cStateWrapper()
        if timeDriftState is None:
            timeDriftState=cStateWrapper()
        if backwardMode is None:
            backwardMode = 1
        # if propagator is None:
        #     dir_path = os.path.dirname(os.path.realpath(__file__))
        #     dllname = dir_path+'\PropagateDLL_0.dll'
        #     libOrbitProp  = cdll.LoadLibrary(dllname)
        #     propagator = libOrbitProp
        self.name = name
        self.kine   = kineState
        self.timeDrift  = timeDriftState
        self.recordFileLength = 500
        self.backwardMode = backwardMode
        self.useTrueProp = False
        # self.propagator = propagator
    def __eq__(self,other):
        try:
            if self.name == other.name and (self.kine.mean == other.kine.mean).all() and (self.kine.cov == other.kine.cov).all() and  (self.timeDrift.mean == other.timeDrift.mean).all() and (self.timeDrift.cov == other.timeDrift.cov).all() and self.curtruetime == other.curtruetime :
                return True
            else:
                return False
        except:
            return False


    def __ne__(self,other):
        return not cSat.__eq__(self,other)
    

    def useTruePropagator(self):
        self.useTrueProp=True

    def initialRecord(self,recordFileprefix =None):
        recordLen  = self.recordFileLength
        self.kineStateMeans = np.zeros((self.kine.dim(),recordLen))
        self.dataIndex  = 1 
        self.fileIndex  = 1
        self.kineStateMeans[:,self.dataIndex-1] = self.kine.mean
        self.kineStateCovs = np.zeros((6,6,recordLen))
        self.kineStateCovs[:,:,self.dataIndex-1] = self.kine.cov
        self.timeTag = np.zeros((recordLen))
        self.timeTag[self.dataIndex-1] = self.curtruetime

        self.timeDiffMeans= np.zeros((recordLen,))
        self.timeDiffMeans[self.dataIndex-1] = self.timeDrift.mean
        self.timeDiffCovs=np.zeros((recordLen,))
        self.timeDiffCovs[self.dataIndex-1]  = self.timeDrift.cov

        if recordFileprefix is None:
            self.recordFileprefix = './data/simResults/'+self.name
        else:
            self.recordFileprefix = recordFileprefix
        folder = os.path.dirname(os.path.relpath(self.recordFileprefix))
        print folder
        if os.path.exists(folder) == False:
            os.makedirs(folder)

    def setRecordProgress(self,fileIndex=1):
        '''
            set record progress, to continue data record
        '''
        self.fileIndex  = fileIndex
    
    def setCurTime(self,time):
        '''
            set Curtime, true time 
            curtruetime is when the state getted
        '''
        bdttime =  time2BDT(time)
        logging.debug(self.name+' says : i am at '+ str(time)+' ('+str(bdttime.weekNum)+ ')'+str(bdttime.SOW))
        self.curtruetime = time

    
    def prop(self,startTime=None,endTime=None):
        '''
            propogate its state given startTime and EndTime. this time is true time
            using the inner state for its state
            both startTime and endTime are float

            return: a wrapper(normalDistributeMsg) 
        '''
        if startTime is None:
            startTime = self.curtruetime
        if endTime  is None:
            raise Exception("must given endtime")
        if startTime == endTime:
            logging.debug(self.name+ ": no need to prop")
            return self.kine
        kineFormed  = self.kine.changeForm()

        BDTstartTime= time2BDT(startTime)
        BDTendTime  = time2BDT(endTime)

        outputData = IntergOutPut()
        formedState   = self.kine.changeForm()
        inputData  = constructIntergInput(BDTstartTime,BDTendTime,formedState[0],formedState[1])
        try:
            LOCK.aquire()
        except:
            pass
        if self.useTrueProp == False:
            LIB_ORBIT_PROP.Integrator_Module(byref(inputData),byref(outputData))
        else:
            LIB_ORBIT_PROP_TRUE.Integrator_Module(byref(inputData),byref(outputData))
        try:
            LOCK.release()
        except:
            pass
        emitKineMsg = createStateWrapperFromPropOutPut(outputData) 
        self.kine  =  emitKineMsg
        self.curtruetime  = endTime
        return emitKineMsg
    def linkedProp(self,linkedSat,linkPara ):
        '''
            the most important part of my code, this code will implement TWO ROUND TIMING
            linkPara['link1endTime']: the true time when satellite emit the first link signal
            linkPara['link1measurement']: the measurement for link1 
            linkPara['link2endTime']:
            linkPara['link2measurement'], similiar to above
        '''
        
        assert(isinstance(linkedSat,cSat))
        g =  cHybridNode('aname',self.backwardMode)
        
        logging.debug("hybrid Construction--->>>>")
        initialStatement = {}
        initialStatement = linkPara
        # add ranging noise
        initialStatement['link1measurement'] = initialStatement['link1measurement']  + np.sqrt(MEARSUREMENTNOISEVAR) * np.random.randn(1).tolist()[0]
        initialStatement['link2measurement'] = initialStatement['link2measurement']  + np.sqrt(MEARSUREMENTNOISEVAR) * np.random.randn(1).tolist()[0]

        initialStatement['link1startTime']  = initialStatement['link1endTime'] -  initialStatement['link1measurement']/LIGHTSPEED
        initialStatement['link2startTime']  = initialStatement['link2endTime'] -  initialStatement['link2measurement']/LIGHTSPEED
        
        # emitKineMsg  == prioriMobilek1 
        initialStatement['emitKineMsg'] =  self.prop(self.curtruetime,initialStatement['link1startTime'])

        initialStatement['recKineMsg']   = linkedSat.prop(linkedSat.curtruetime,initialStatement['link1endTime'])
        
        initialStatement['emitPosMsg']    =  initialStatement['emitKineMsg'].partState(0,2)
        initialStatement['recPosMsg']    =  initialStatement['recKineMsg'].partState(0,2)
        
        initialStatement['emitTimeDriftMsg'] =  self.timeDrift
        initialStatement['recTimeDriftMsg'] =  linkedSat.timeDrift
        logging.debug("timeDiff on the beginning:%s: %s ;\n %s: %s"%(self.name,self.timeDrift,linkedSat.name,linkedSat.timeDrift) )
        initialStatement['emitSat']       = self
        initialStatement['recSat']        = linkedSat
        initialStatement['emitSatName']       = self.name
        initialStatement['recSatName']        = linkedSat.name

        graphConstructPara = {'hasMutualMeasure':True,
                              'initialStatement': initialStatement}

        g.constructGraph(graphConstructPara)

        logging.debug("hybrid Construction DONE---<<<<<") 
        results  =  g.run(1)
        results['link2startTime'] = initialStatement['link2startTime']

        #BUG: there is no timediff data assignment 
        self.setCurTime(initialStatement['link2endTime'])
        oldkinetrace = np.trace(self.kine.cov)
        self.kine = results['sat1KineState']
        newtrace     = np.trace(self.kine.cov)
        logging.info('the %s trace chanege: %f '%(self.name,newtrace-oldkinetrace))
        self.giveResultsto(linkedSat,results)
    def giveResultsto(self,linkedSat,results):
        '''

        '''
        #BUG: there is no timediff data assignment
        linkedSat.setCurTime(results['link2startTime'])
        oldkinetrace = np.trace(linkedSat.kine.cov)
        linkedSat.kine = results['sat2KineState']
        newtrace     = np.trace(linkedSat.kine.cov)
        logging.info('(receive sat)the %s trace chanege: %f '%(linkedSat.name,newtrace-oldkinetrace))
    
    def recordNow(self):
        self.dataIndex  += 1 
        self.timeTag[self.dataIndex-1] = self.curtruetime
        self.kineStateCovs[:,:,self.dataIndex-1] = self.kine.cov
        self.kineStateMeans[:,self.dataIndex-1] = self.kine.mean
        self.timeDiffMeans[self.dataIndex-1] = self.timeDrift.mean
        self.timeDiffCovs[self.dataIndex-1]  = self.timeDrift.cov
        if self.dataIndex == self.recordFileLength:
            self.dumpRecords()
        elif self.dataIndex > self.recordFileLength:
            raise Exception('something wrong')
    
    def dumpRecords(self,filename=None):
        if filename is None:
            filename = self.recordFileprefix+'_'+ str(self.fileIndex ) +'.npz'
        np.savez(filename,timeTag= self.timeTag[0:self.dataIndex],kineStateMeans = self.kineStateMeans[:,0:self.dataIndex], kineStateCovs=self.kineStateCovs[:,:,0:self.dataIndex],timeDiffMeans=self.timeDiffMeans[0:self.dataIndex],timeDiffCovs=self.timeDiffCovs[0:self.dataIndex])
        self.fileIndex  =self.fileIndex + 1
        self.dataIndex = 0


if __name__ == '__main__':
    
    import dataread
    import satellite

    data = dataRead()
    data.fetchdata()
    loadProgressFile = './data/progressData/defaultprogress.data'
    progressData =  data.readProgressData(loadProgressFile)

    iterationNum =  50000
    satCurrentTime  = 307627203 
    
    sats = []
    for i in range(0,1):
        satInitState =     progressData['satInitState'][i]
        satInitKineMean  = np.array((16983352.634205 ,    -20353708.086466        ,    -6.183084 ,      1709.449900   ,    1426.384892     ,  3179.607493),dtype=float)
        # satInitKineMean    = np.array(data.initialData[i][1:7],dtype=float)
        satInitKineCov   = np.array(np.zeros((6,6)),dtype=float)
        # satInitKineCov  = np.array(DEFAULTINITPSE,dtype=float)
        satTimeDrift     = progressData['satTimeDrift'][i]
        satInitKineSate  = cStateWrapper(satInitKineMean,satInitKineCov)
        timeDrift        = cStateWrapper(np.array(satTimeDrift['mean'],dtype=float),np.array(satTimeDrift['cov'],dtype=float))
        sat              = cSat('sat'+str(i+1),satInitKineSate,timeDrift)
        sat.setCurTime(satCurrentTime)
        sat.recordFileLength = 10000
        sat.initialRecord('./data/simResults/3/'+sat.name)
        sat.setRecordProgress()
        
        sats.append(sat) # satnum is index +1
        
    for i in range(0,1):
        print 'sat recording'
        sat = sats[i]
        for j in range(0,iterationNum):
            sat.prop(sat.curtruetime,sat.curtruetime+100)
            sat.recordNow()